%
%***********************************************************************

% Date: 2013-05-26  Time: 22:02:34

%************************ CADAC UTILITIES ******************************
%***************************** UTL.FOR *********************************
%***********************************************************************
%***  *
%***  * MATxxx, VECxxx, TABLx,and CADxxx Utility Subroutines.
%***  *
%***  * MODIFICATION HISTORY
%***  * 970430 Modified VECANG to prevent rare math overflow problem, PZi
%***  * 970501 Version 2.0, Released with CADX2.FOR, Version 2
%***  * 970619 Corrections in MATADD and MATSUB, PZi
%***  * 980529 RANF Function added for RANF calls with FNGAUSS routine, bc
%***  * 991122 Added new table look-up TBLPx routines with pointers, PZi
%***  * 000204 Added subroutine US76 std atmosphere, PZi
%***  * 000412 Included CADxx utilities from UTL8.FOR, renamed UTL3.FOR, PZi
%***  *
%***  *****************************************************************


function [c,a,b,n1,n2]=matadd(c,a,b,n1,n2);

%*** C(N1*N2) IS OBTAINED FROM ADDING A(N1*N2) TO B(N1*N2)

function [c,a,b,n1,n2]=matsub(c,a,b,n1,n2);

%*** C(N1*N2) IS OBTAINED FROM SUBSTRACTING B(N1*N2) FROM A(N1*N2)

function [c,a,b,n1,n2,n3]=matmul(c,a,b,n1,n2,n3);

%*** C(N1*N3) IS OBTAINED FROM MULTIPLYING A(N1*N2) BY B(N2*N3)

function [d,v1,v2,n]=matsca(d,v1,v2,n);

%*** D IS THE SCALAR PRODUCT OF VECTOR V1(N*1) AND V2(N*1)



%          SUBROUTINE MATINV

%          PURPOSE
%             COMPUTE THE INVERSE OF A NON-SINGLAR MATRIX

%          USAGE
%             CALL MATINV(AI,A,D,N)

%          DESCRIPTION OF PARAMETERS
%             A - NAME OF INPUT MATRIX
%             AI- NAME OF OUTPUT MATRIX (MAY BE THE SAME AS A)
%             D - DETERMINANT OF A
%             N - NUMBER OF ROWS AND COLUMNS OF A

%          REMARKS
%             ALL MATRICES ARE STORED AS GENERAL MATRICES

%          SUBROUTINES AND FUNCTION SUBPROGRAMS RQUIRED
%             SYSTEM ROUTINE -MATRIX-

%          METHOD
%        SET UP TO CALL GAUSS-JORDAN INVERSION ROUTINE.

%     ..................................................................

function [ai,a,d,n]=matinv(ai,a,d,n);

function [epsil,mx,m,a,ier,det]=matfs(epsil,mx,m,a,ier,det);
%      MATFS IS A SINGLE-PRECISION MATRIX INVERSION SUBROUTINE
%     WRITTEN IN FORTRAN IV  - VARIABLE DIMENSIONING IS USED
%     OF MAX DIMENSIONS OF MATRIX IN CSLLING PROGRAM MUST BE SAME
%     VALUE AS PASSED ON BY CALLING SEQUENCE AS MX.
%     MATRIX CAN BE OF GENERAL FORM. METHOD IS GAUSS-JORDAN.

function [c,con,a,n1,n2]=matcon(c,con,a,n1,n2);

%*** C(N1*N2) IS OBTAINED FROM MULTIPLYING COSTANT CON BY A(N1*N2)

function [c,a,n1,n2]=mattra(c,a,n1,n2);

%*** C(N2*N1) IS THE TRANSPOSE OF A(N1*N2)

function [c,t,a,n]=mattrf(c,t,a,n);

%*** C(N*N) IS OBTAINED FROM A(N*N) BY THE ORTHOGONAL TRANSFORMATION
%    MATRIX T(N*N)    C = T * A * T(TRANSPOSE)


function [c,v]=matsks(c,v);

%*** SKEW-SYMMETRIC MATRIX C(3*3) IS OBTAINED FROM VECTOR C(3*1)

function [v,a]=matvsk(v,a);

%*** VECTOR V(3*1) IS OBTAINED FROM SKEW-SYMMETRIC MATRIX A(3*3)

a_orig=a;a_shape=[3,3];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);


v(1)=-a(2,3);
v(2)=a(1,3);
v(3)=-a(1,2);

a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
end %subroutine matvsk


function [a,n1,n2]=matzer(a,n1,n2);

%*** CREATION OF A ZERO MATRIX A(N1*N2)

function [a,n]=matuni(a,n);

%% Matlab: I = eye(n) returns an n-by-n identity matrix with ones on the main diagonal and zeros elsewhere.
%*** CREATION OF A UNIT MATRIX A(N*N) 



function [a,d,n]=matdia(a,d,n);
%*** A(N*N) IS THE DIAGONAL MATRIX OBTAINED FROM VECTOR D(N*1)

%% Matlab: X = diag(v,k)


a_orig=a;a_shape=[n,n];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);


for  j=1:n;
  for  i=1:n;
    a(i,j)=0.;
  end; 
  i=n+1;
  a(j,j)=d(j);
end;  

j=n+1;
a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));
a=a_orig;

return;
end %subroutine matdia


function [d,a,n]=matvdi(d,a,n);

%*** VECTOR D(N*1) IS THE DIAGONAL OF MATRIX A(N*N)




a_orig=a;a_shape=[n,n];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);


for  i=1:n;
d(i)=a(i,i);
end;  i=n+1;

a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
end %subroutine matvdi

function [c,a,n1,n2]=mateql(c,a,n1,n2);

%*** C(N1*N2) IS SET EQUAL TO A(N1*N2)






l=n1.*n2;
for  i=1:l;
c(i)=a(i);
end;  i=l+1;
return;
end %subroutine mateql

function [d,v,n]=matabs(d,v,n);

%*** D IS THE ABSOLUTE VALUE OF VECTOR V(N*1)






dv=0.;
for  i=1:n;
dv=dv+v(i).*v(i);
end;  i=n+1;
d=sqrt(dv);
return;
end %subroutine matabs

function [sbel,dbe,azbel,elbel]=matcar(sbel,dbe,azbel,elbel);

%*** CARTESIAN COORDINATES SBEL(3X1) FROM POLAR COORDINATES






celb=cos(elbel);
selb=sin(elbel);
cazb=cos(azbel);
sazb=sin(azbel);

sbel(1)=dbe.*celb.*cazb;
sbel(2)=dbe.*celb.*sazb;
sbel(3)=-dbe.*selb;

return;
end %subroutine matcar

function [dbe,azbel,elbel,sbel]=matpol(dbe,azbel,elbel,sbel);

%*** POLAR COORDINATES FROM CARTESIAN COORDINATES

% Replacement matlab function!!!!
%pol2cart
%Transform polar or cylindrical coordinates to Cartesian

%Syntax
%[X,Y] = pol2cart(THETA,RHO)
%[X,Y,Z] = pol2cart(THETA,RHO,Z)

%% Also see [THETA,RHO,Z] = cart2pol(X,Y,Z)
%% Also see [azimuth,elevation,r] = cart2sph(X,Y,Z)





if(max(abs(sbel(1)),abs(sbel(2))) < 1e-10)
azbel=0.;
elbel=0.;
dum1=0.;
else;
azbel=atan2(sbel(2),sbel(1));
dum1=sbel(1).*sbel(1)+sbel(2).*sbel(2);
dum2=sqrt(dum1);
elbel=atan2(-sbel(3),dum2);
end;
dum3=dum1+sbel(3).*sbel(3);
dbe=sqrt(dum3);

return;
end %subroutine matpol

function [t,psi,tht]=mat2tr(t,psi,tht);

%*** T IS THE TRANSFORMATION MATRIX OBTAINED FROM TWO
%*** ANGLE TRANSFORMATIONS PSI AND THT




t_orig=t;t_shape=[3,3];t=reshape([t_orig(1:min(prod(t_shape),numel(t_orig))),zeros(1,max(0,prod(t_shape)-numel(t_orig)))],t_shape);


t(1,3)=-sin(tht);
t(2,1)=-sin(psi);
t(2,2)=cos(psi);
t(3,3)=cos(tht);
t(1,1)=t(3,3).*t(2,2);
t(1,2)=-t(3,3).*t(2,1);
t(3,1)=-t(1,3).*t(2,2);
t(3,2)=t(1,3).*t(2,1);
t(2,3)=0.;

t_orig(1:min(prod(t_shape),numel(t_orig)))=t(1:min(prod(t_shape),numel(t_orig)));t=t_orig;
return;
end %subroutine mat2tr

function [t,psi,tht,phi]=mat3tr(t,psi,tht,phi);

%*** T IS THE TRANSFORMATION MATRIX OBTAINED FROM THREE
%*** ANGLE TRANSFORMATIONS PSI, THT AND PHI (IN THIS SEQUENCE)
%*** EULER ANGLES OF FLIGHT MECHANICS




t_orig=t;t_shape=[3,3];t=reshape([t_orig(1:min(prod(t_shape),numel(t_orig))),zeros(1,max(0,prod(t_shape)-numel(t_orig)))],t_shape);


cpsi=cos(psi);
spsi=sin(psi);
ctht=cos(tht);
stht=sin(tht);
cphi=cos(phi);
sphi=sin(phi);
t(1,1)=cpsi.*ctht;
t(1,2)=spsi.*ctht;
t(1,3)=-stht;
t(2,1)=-spsi.*cphi+cpsi.*stht.*sphi;
t(2,2)=cpsi.*cphi+spsi.*stht.*sphi;
t(2,3)=ctht.*sphi;
t(3,1)=spsi.*sphi+cpsi.*stht.*cphi;
t(3,2)=-cpsi.*sphi+spsi.*stht.*cphi;
t(3,3)=ctht.*cphi;

t_orig(1:min(prod(t_shape),numel(t_orig)))=t(1:min(prod(t_shape),numel(t_orig)));t=t_orig;
return;
end %subroutine mat3tr

function [t,psi,alam,ome]=mat3eu(t,psi,alam,ome);

%*** TRANSF. MATRIX OF THE THREE EULER ANGLES PSI,ALAM,OME (IN THIS SEQ.)
%*** EULER ANGLES OF GYRODYNAMICS, NOT FLIGHT MECHANICS.




t_orig=t;
t_shape=[3,3];
t=reshape([t_orig(1:min(prod(t_shape),numel(t_orig))),zeros(1,max(0,prod(t_shape)-numel(t_orig)))],t_shape);


come=cos(ome);
some=sin(ome);
calam=cos(alam);
salam=sin(alam);
cpsi=cos(psi);
spsi=sin(psi);

sc=some.*calam;
cc=come.*calam;

t(1,1)=come.*cpsi-sc.*spsi;
t(1,2)=come.*spsi+sc.*cpsi;
t(1,3)=some.*salam;
t(2,1)=-some.*cpsi-cc.*spsi;
t(2,2)=-some.*spsi+cc.*cpsi;
t(2,3)=come.*salam;
t(3,1)=salam.*spsi;
t(3,2)=-salam.*cpsi;
t(3,3)=calam;

t_orig(1:min(prod(t_shape),numel(t_orig)))=t(1:min(prod(t_shape),numel(t_orig)));

t=t_orig;
return;
end %subroutine mat3eu

function [r,a,b]=matrot(r,a,b);

%*** ROTATION TENSOR R(3X3) FROM ANGLE OF ROTATION A AND
%    VECTOR OF ROTATION B(3X1)




r_orig=r;r_shape=[3,3];r=reshape([r_orig(1:min(prod(r_shape),numel(r_orig))),zeros(1,max(0,prod(r_shape)-numel(r_orig)))],r_shape);


ca=cos(a);
sa=sin(a);
oca=1.-ca;
b12oca=b(1).*b(2).*oca;
b13oca=b(1).*b(2).*oca;
b23oca=b(2).*b(3).*oca;

r(1,1)=ca+b(1).*b(1).*oca;
r(1,2)=b12oca-b(3).*sa;
r(1,3)=b13oca+b(2).*sa;
r(2,1)=b12oca+b(3).*sa;
r(2,2)=ca+b(2).*b(2).*oca;
r(2,3)=b23oca-b(1).*sa;
r(3,1)=b13oca-b(2).*sa;
r(3,2)=b23oca+b(1).*sa;
r(3,3)=ca+b(3).*b(3).*oca;

r_orig(1:min(prod(r_shape),numel(r_orig)))=r(1:min(prod(r_shape),numel(r_orig)));r=r_orig;
return;
end %subroutine matrot

%*** CHOLESKY DECOMPOSITION. LOWER TRIANGULAR MATRIX A(N,N) WHICH IS THE
%    SQUARE ROOT OF MATRIX P(N,N)

function [a,p,n]=matcho(a,p,n);




a_orig=a;a_shape=[n,n];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);
p_orig=p;p_shape=[n,n];p=reshape([p_orig(1:min(prod(p_shape),numel(p_orig))),zeros(1,max(0,prod(p_shape)-numel(p_orig)))],p_shape);


for  i=1:n;
for  j=1:n;

if(j < i)
sum=0.;
if(j > 1)
loop20:  do  k=1,j-1;
sum=sum+a(i,k).*a(j,k);
end;
if(a(j,j) == 0.)
a(i,j)=0.;
else;
a(i,j)=(p(i,j)-sum)./a(j,j);
end;

elseif(j == i);
sum=0.;
if(i > 1)
for  k=1:i-1;
sum=sum+a(i,k).^2;
end;  k=i-1+1;
a(i,j)=sqrt(p(i,i)-sum);

else;
a(i,j)=0.;

end;

% 10 continue;

a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
p_orig(1:min(prod(p_shape),numel(p_orig)))=p(1:min(prod(p_shape),numel(p_orig)));p=p_orig;
return;
end %subroutine matcho;